#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include "KeyPoseFrame.h"
#include "PoseFrame.h"

#include <math.h>

#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
#include "Thirdparty/g2o/g2o/core/sparse_block_matrix.h"
#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"

namespace msf
{
class Optimizer
{
public:
/*
      ORB-SLAM2优化分为四种：
      （1）全局BA优化（在单目初始化之后、回环检测之后进行全局BA优化。对所有关键帧的位姿、路标点进行优化）
      （2）局部BA优化（在局部建图之后进行，对当前帧及其共视关键帧的位姿、路标点进行优化）
      （3）位姿图优化（仅优化位姿，在恒速运动模型、参考关键字模型、重定位、局部地图跟踪的时候执行）
      （4）本质图优化（在回环校正过程中进行优化，将闭环误差分配到本质图中）
      （5）Sim3优化（检测到回环之后，会进行闭环帧和当前帧间的Sim3估计，随后进行Sim3优化）
*/

//初始化
      void static InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, Eigen::MatrixXd  &covInertial, bool bFixedVel=false, bool bGauss=false, float priorG = 1e2, float priorA = 1e6);
      //void static InertialOptimization(Map *pMap, Eigen::Vector3d &bg, Eigen::Vector3d &ba, float priorG = 1e2, float priorA = 1e6);

      void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKPF=0, bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL);

//tracking
      //int static PoseOptimization(PoseFrame* pPoseFrame);
      int static PoseInertialOptimization(PoseFrame *pPoseFrame, bool bRecInit = false);
      // tracklocalmap
      int static PoseInertialOptimizationLastPoseFrame(PoseFrame *pPoseFrame, bool bRecInit = false);
      int static PoseInertialOptimizationLastKeyPoseFrame(PoseFrame* pPoseFrame, bool bRecInit = false);
//localmapping
      void static LocalBundleAdjustment(KeyPoseFrame* pKPF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges);
      void static LocalInertialBA(KeyPoseFrame* pKPF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bRecInit = false);

      // Marginalize block element (start:end,start:end). Perform Schur complement.
      // Marginalized elements are filled with zeros.
      static Eigen::MatrixXd Marginalize(const Eigen::MatrixXd &H, const int &start, const int &end);

      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

} // namespace msf

#endif // OPTIMIZER_H